Hierarchical Hybrid Control with Classical Planning and Trajectory Optimization

One method of control of cyberphysical systems, particularly intelligent mobile robots, involves a hierarchy of high-level classical planning and low-level trajectory optimization. We present a new technique to incorporate trajectory costs in a high-level classical planning problem (CPP). The proposed approach is particularly suited to leverage existing algorithms for solving CPPs and, independently, algorithms for trajectory optimization. To this end, we introduce a family of graphs called lifted planning graphs parametrized by an integer H, and we map paths in these graphs to solutions of the CPP. We show that the overall cost of a high-level plan is a nonincreasing function of H, and that there exists a finite H for which an optimal path in the lifted planning graph is associated with the optimal solution of the CPP. For computational speed and future real-time implementation, we discuss incremental modification of paths in the lifted planning graphs for increasing values of H. We illustrate the proposed ideas with a numerical simulation example involving classical planning for Dubins vehicle routing.